
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_spline_curve.h
  * @author     baiyang
  * @date       2022-9-6
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "gp_mathlib.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct spline_curve* spline_curve_t;

/** @ 
  * @brief  
  */
struct spline_curve {
    // interval variables
    Vector3f_t    _origin;                // origin offset (in NEU frame) from EKF
    Vector3f_t    _destination;           // destination offset (in NEU frame) from EKF
    Vector3f_t    _origin_vel;            // the target velocity vector in NEU frame at the origin of the spline segment
    Vector3f_t    _destination_vel;       // the target velocity vector in NEU frame at the destination point of the spline segment
    Vector3f_t    _hermite_solution[4];   // array describing path between origin and destination
    float         _time;                  // current spline time (between 0 and 1) between origin and destination
    float         _speed_xy;              // maximum horizontal speed
    float         _speed_up;              // maximum speed upwards
    float         _speed_down;            // maximum speed downwards
    float         _accel_xy;              // maximum horizontal acceleration
    float         _accel_z;               // maximum vertical acceleration
    float         _origin_speed_max;      // maximum speed at origin
    float         _destination_speed_max; // maximum speed at destination
    bool          _reached_destination;   // true once vehicle has reached destination
    bool          _zero_length;           // true if spline is zero length
};

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void slcurve_ctor(spline_curve_t scurve);

// set maximum speed and acceleration
void slcurve_set_speed_accel(spline_curve_t scurve, float speed_xy, float speed_up, float speed_down,
                                  float accel_xy, float accel_z);

// set origin and destination using position vectors (offset from EKF origin)
// origin_vel is vehicle velocity at origin (in NEU frame)
// destination_vel is vehicle velocity at destination (in NEU frame)
// time is reset to zero
void slcurve_set_origin_and_destination(spline_curve_t scurve, const Vector3f_t *origin, const Vector3f_t *destination, const Vector3f_t *origin_vel, const Vector3f_t *destination_vel);

// move target location along track from origin to destination
// target_pos is updated with the target position from EKF origin in NEU frame
// target_vel is updated with the target velocity in NEU frame
void slcurve_advance_target_along_track(spline_curve_t scurve, float dt, Vector3f_t *target_pos, Vector3f_t *target_vel);

// returns true if vehicle has reached destination
static inline bool slcurve_reached_destination(spline_curve_t scurve) { return scurve->_reached_destination; }

// returns the unscaled destination velocity vector
static inline const Vector3f_t slcurve_get_destination_vel(spline_curve_t scurve) { return scurve->_destination_vel; }

// returns maximum speed at origin
static inline float slcurve_get_origin_speed_max(spline_curve_t scurve) { return scurve->_origin_speed_max; }

// get or set maximum speed at destination
static inline float slcurve_get_destination_speed_max(spline_curve_t scurve) { return scurve->_destination_speed_max; }
static inline void slcurve_set_destination_speed_max(spline_curve_t scurve, float destination_speed_max) { scurve->_destination_speed_max = MIN(scurve->_destination_speed_max, destination_speed_max); }

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



